<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN" "http://www.w3.org/TR/html4/loose.dtd">
<html lang="en">
<head>
<title>Source code</title>
<link rel="stylesheet" type="text/css" href="../../../../stylesheet.css" title="Style">
</head>
<body>
<div class="sourceContainer">
<pre><span class="sourceLineNo">001</span>package frc3128.HardwareLink.Motor;<a name="line.1"></a>
<span class="sourceLineNo">002</span><a name="line.2"></a>
<span class="sourceLineNo">003</span>import edu.wpi.first.wpilibj.Jaguar;<a name="line.3"></a>
<span class="sourceLineNo">004</span>import frc3128.HardwareLink.Encoder.AbstractEncoder;<a name="line.4"></a>
<span class="sourceLineNo">005</span>import frc3128.Util.Constants;<a name="line.5"></a>
<span class="sourceLineNo">006</span>import frc3128.Util.DebugLog;<a name="line.6"></a>
<span class="sourceLineNo">007</span>import frc3128.Util.RobotMath;<a name="line.7"></a>
<span class="sourceLineNo">008</span><a name="line.8"></a>
<span class="sourceLineNo">009</span>/**<a name="line.9"></a>
<span class="sourceLineNo">010</span> * <a name="line.10"></a>
<span class="sourceLineNo">011</span> * @author Noah Sutton-Smolin<a name="line.11"></a>
<span class="sourceLineNo">012</span> */<a name="line.12"></a>
<span class="sourceLineNo">013</span>public final class MotorLink {<a name="line.13"></a>
<span class="sourceLineNo">014</span>    private final Jaguar            jag;<a name="line.14"></a>
<span class="sourceLineNo">015</span>    private       AbstractEncoder   encoder;<a name="line.15"></a>
<span class="sourceLineNo">016</span>    private       MotorSpeedControl spdControl;<a name="line.16"></a>
<span class="sourceLineNo">017</span>    private       boolean           spdControlEnabled = false;<a name="line.17"></a>
<span class="sourceLineNo">018</span>    private       boolean           motorReversed = false;<a name="line.18"></a>
<span class="sourceLineNo">019</span>    private       double            powerScalar = 1;<a name="line.19"></a>
<span class="sourceLineNo">020</span>    <a name="line.20"></a>
<span class="sourceLineNo">021</span>    /**<a name="line.21"></a>
<span class="sourceLineNo">022</span>     * <a name="line.22"></a>
<span class="sourceLineNo">023</span>     * @param motor the linked motor<a name="line.23"></a>
<span class="sourceLineNo">024</span>     */<a name="line.24"></a>
<span class="sourceLineNo">025</span>    public MotorLink(Jaguar motor) {this.jag = motor;}<a name="line.25"></a>
<span class="sourceLineNo">026</span>    <a name="line.26"></a>
<span class="sourceLineNo">027</span>    /**<a name="line.27"></a>
<span class="sourceLineNo">028</span>     * <a name="line.28"></a>
<span class="sourceLineNo">029</span>     * @param motor the linked motor<a name="line.29"></a>
<span class="sourceLineNo">030</span>     * @param enc the linked encoder<a name="line.30"></a>
<span class="sourceLineNo">031</span>     */<a name="line.31"></a>
<span class="sourceLineNo">032</span>    public MotorLink(Jaguar motor, AbstractEncoder enc) {this(motor); this.encoder = enc;}<a name="line.32"></a>
<span class="sourceLineNo">033</span>    <a name="line.33"></a>
<span class="sourceLineNo">034</span>    /**<a name="line.34"></a>
<span class="sourceLineNo">035</span>     * <a name="line.35"></a>
<span class="sourceLineNo">036</span>     * @param motor the linked motor<a name="line.36"></a>
<span class="sourceLineNo">037</span>     * @param enc the linked encoder<a name="line.37"></a>
<span class="sourceLineNo">038</span>     * @param spdControl the linked speed controller<a name="line.38"></a>
<span class="sourceLineNo">039</span>     */<a name="line.39"></a>
<span class="sourceLineNo">040</span>    public MotorLink(Jaguar motor, AbstractEncoder enc, MotorSpeedControl spdControl) {this(motor, enc); this.spdControl = spdControl;}<a name="line.40"></a>
<span class="sourceLineNo">041</span><a name="line.41"></a>
<span class="sourceLineNo">042</span>    /**<a name="line.42"></a>
<span class="sourceLineNo">043</span>     * <a name="line.43"></a>
<span class="sourceLineNo">044</span>     * @param motor the linked motor<a name="line.44"></a>
<span class="sourceLineNo">045</span>     * @param powerScalar the starting power scalar of the motor<a name="line.45"></a>
<span class="sourceLineNo">046</span>     */<a name="line.46"></a>
<span class="sourceLineNo">047</span>    public MotorLink(Jaguar motor, double powerScalar) {this(motor); this.setPowerScalar(powerScalar);}<a name="line.47"></a>
<span class="sourceLineNo">048</span><a name="line.48"></a>
<span class="sourceLineNo">049</span>    /**<a name="line.49"></a>
<span class="sourceLineNo">050</span>     * <a name="line.50"></a>
<span class="sourceLineNo">051</span>     * @param motor the linked motor<a name="line.51"></a>
<span class="sourceLineNo">052</span>     * @param enc the linked encoder<a name="line.52"></a>
<span class="sourceLineNo">053</span>     * @param powerScalar the starting power scalar of the motor<a name="line.53"></a>
<span class="sourceLineNo">054</span>     */<a name="line.54"></a>
<span class="sourceLineNo">055</span>    public MotorLink(Jaguar motor, AbstractEncoder enc, double powerScalar) {this(motor, enc); this.setPowerScalar(powerScalar);}<a name="line.55"></a>
<span class="sourceLineNo">056</span>    <a name="line.56"></a>
<span class="sourceLineNo">057</span>    /**<a name="line.57"></a>
<span class="sourceLineNo">058</span>     * <a name="line.58"></a>
<span class="sourceLineNo">059</span>     * @param motor the linked motor<a name="line.59"></a>
<span class="sourceLineNo">060</span>     * @param enc the linked encoder<a name="line.60"></a>
<span class="sourceLineNo">061</span>     * @param spdControl the linked speed controller<a name="line.61"></a>
<span class="sourceLineNo">062</span>     * @param powerScalar the starting power scalar of the motor<a name="line.62"></a>
<span class="sourceLineNo">063</span>     */<a name="line.63"></a>
<span class="sourceLineNo">064</span>    public MotorLink(Jaguar motor, AbstractEncoder enc, MotorSpeedControl spdControl, double powerScalar) {this(motor, enc, spdControl); this.setPowerScalar(powerScalar);}<a name="line.64"></a>
<span class="sourceLineNo">065</span>    <a name="line.65"></a>
<span class="sourceLineNo">066</span>    /**<a name="line.66"></a>
<span class="sourceLineNo">067</span>     * <a name="line.67"></a>
<span class="sourceLineNo">068</span>     * @return the current speed of the motor<a name="line.68"></a>
<span class="sourceLineNo">069</span>     */<a name="line.69"></a>
<span class="sourceLineNo">070</span>    public double getSpeed() {return jag.get();}<a name="line.70"></a>
<span class="sourceLineNo">071</span>    <a name="line.71"></a>
<span class="sourceLineNo">072</span>    /**<a name="line.72"></a>
<span class="sourceLineNo">073</span>     * Sets the speed of the motor. <a name="line.73"></a>
<span class="sourceLineNo">074</span>     * <a name="line.74"></a>
<span class="sourceLineNo">075</span>     * @param spd the speed to be set<a name="line.75"></a>
<span class="sourceLineNo">076</span>     */<a name="line.76"></a>
<span class="sourceLineNo">077</span>    public void setSpeed(double spd) {<a name="line.77"></a>
<span class="sourceLineNo">078</span>        if(spdControlEnabled) {<a name="line.78"></a>
<span class="sourceLineNo">079</span>            DebugLog.log(DebugLog.LVL_SEVERE, this, "The speed of " + this + " was set while a controller was active! Deleting speed controller...");<a name="line.79"></a>
<span class="sourceLineNo">080</span>            this.deleteSpeedControl();<a name="line.80"></a>
<span class="sourceLineNo">081</span>        }<a name="line.81"></a>
<span class="sourceLineNo">082</span>        if(spd &lt; -1 || spd &gt; 1) spd = (spd &lt; 0 ? -1 : 1);<a name="line.82"></a>
<span class="sourceLineNo">083</span>        <a name="line.83"></a>
<span class="sourceLineNo">084</span>        this.jag.set(reversedCheck(spd)*this.powerScalar);<a name="line.84"></a>
<span class="sourceLineNo">085</span>    }<a name="line.85"></a>
<span class="sourceLineNo">086</span>    protected void setSpeedControlled(double spd) {this.jag.set(spd);}<a name="line.86"></a>
<span class="sourceLineNo">087</span>    <a name="line.87"></a>
<span class="sourceLineNo">088</span>    /**<a name="line.88"></a>
<span class="sourceLineNo">089</span>     * Sets and enables an encoder to be linked with a motor.<a name="line.89"></a>
<span class="sourceLineNo">090</span>     * <a name="line.90"></a>
<span class="sourceLineNo">091</span>     * @param encoder the Encoder to be linked<a name="line.91"></a>
<span class="sourceLineNo">092</span>     */<a name="line.92"></a>
<span class="sourceLineNo">093</span>    public void setEncoder(AbstractEncoder encoder) {<a name="line.93"></a>
<span class="sourceLineNo">094</span>        if(encoder != null) DebugLog.log(DebugLog.LVL_WARN, this, "The encoder was changed in " + this + "!");<a name="line.94"></a>
<span class="sourceLineNo">095</span>        this.encoder = encoder;<a name="line.95"></a>
<span class="sourceLineNo">096</span>    }<a name="line.96"></a>
<span class="sourceLineNo">097</span><a name="line.97"></a>
<span class="sourceLineNo">098</span>    /**<a name="line.98"></a>
<span class="sourceLineNo">099</span>     * <a name="line.99"></a>
<span class="sourceLineNo">100</span>     * @return the current encoder angle<a name="line.100"></a>
<span class="sourceLineNo">101</span>     */<a name="line.101"></a>
<span class="sourceLineNo">102</span>    public double getEncoderAngle() {<a name="line.102"></a>
<span class="sourceLineNo">103</span>        if(encoder == null) {DebugLog.log(DebugLog.LVL_SEVERE, this, "The encoder heading was read, but no encoder was set!"); return -1;}<a name="line.103"></a>
<span class="sourceLineNo">104</span>        return RobotMath.normalizeAngle(encoder.getAngle());<a name="line.104"></a>
<span class="sourceLineNo">105</span>    }<a name="line.105"></a>
<span class="sourceLineNo">106</span>    <a name="line.106"></a>
<span class="sourceLineNo">107</span>    /**<a name="line.107"></a>
<span class="sourceLineNo">108</span>     * Clears, then sets a speed controller for the motor. The motor must have<a name="line.108"></a>
<span class="sourceLineNo">109</span>     * an active encoder.<a name="line.109"></a>
<span class="sourceLineNo">110</span>     * <a name="line.110"></a>
<span class="sourceLineNo">111</span>     * @param spdControl the speed controller to be enabled on the motor<a name="line.111"></a>
<span class="sourceLineNo">112</span>     */<a name="line.112"></a>
<span class="sourceLineNo">113</span>    public void loadSpeedControl(MotorSpeedControl spdControl) {<a name="line.113"></a>
<span class="sourceLineNo">114</span>        if(this.encoder == null) {DebugLog.log(DebugLog.LVL_ERROR, this, "Cannot set the speed controller without an encoder!"); throw new Error();}<a name="line.114"></a>
<span class="sourceLineNo">115</span>        if(spdControlEnabled) {<a name="line.115"></a>
<span class="sourceLineNo">116</span>            DebugLog.log(DebugLog.LVL_SEVERE, this, "The speed controller was set while another was active!");<a name="line.116"></a>
<span class="sourceLineNo">117</span>            this.jag.set(0); this.deleteSpeedControl();<a name="line.117"></a>
<span class="sourceLineNo">118</span>        }<a name="line.118"></a>
<span class="sourceLineNo">119</span>        this.spdControl = spdControl; this.spdControl.clearControlRun(); <a name="line.119"></a>
<span class="sourceLineNo">120</span>        this.spdControlEnabled = true; spdControl.registerIterableEvent();<a name="line.120"></a>
<span class="sourceLineNo">121</span>    }<a name="line.121"></a>
<span class="sourceLineNo">122</span>    <a name="line.122"></a>
<span class="sourceLineNo">123</span>    /**<a name="line.123"></a>
<span class="sourceLineNo">124</span>     * Deletes the existing speed controller.<a name="line.124"></a>
<span class="sourceLineNo">125</span>     */<a name="line.125"></a>
<span class="sourceLineNo">126</span>    public void deleteSpeedControl() {<a name="line.126"></a>
<span class="sourceLineNo">127</span>        if(spdControl == null) {DebugLog.log(DebugLog.LVL_SEVERE, this, "The speed controller was stopped when it did not exist!"); return;}<a name="line.127"></a>
<span class="sourceLineNo">128</span>        if(!this.spdControlEnabled) DebugLog.log(DebugLog.LVL_WARN, this, "The speed controller was disabled when it was not active!");<a name="line.128"></a>
<span class="sourceLineNo">129</span>        this.jag.set(0); this.spdControl.clearControlRun();<a name="line.129"></a>
<span class="sourceLineNo">130</span>        this.spdControlEnabled = false; spdControl.cancelEvent();<a name="line.130"></a>
<span class="sourceLineNo">131</span>    }<a name="line.131"></a>
<span class="sourceLineNo">132</span>    <a name="line.132"></a>
<span class="sourceLineNo">133</span>    /**<a name="line.133"></a>
<span class="sourceLineNo">134</span>     * Enables the speed controller on the current motor with the given starting<a name="line.134"></a>
<span class="sourceLineNo">135</span>     * value.<a name="line.135"></a>
<span class="sourceLineNo">136</span>     * <a name="line.136"></a>
<span class="sourceLineNo">137</span>     * @param targetVal the value to be passed to the speed controller<a name="line.137"></a>
<span class="sourceLineNo">138</span>     */<a name="line.138"></a>
<span class="sourceLineNo">139</span>    public void startSpeedControl(double targetVal) {<a name="line.139"></a>
<span class="sourceLineNo">140</span>        this.spdControl.setControlTarget(targetVal);<a name="line.140"></a>
<span class="sourceLineNo">141</span>        this.spdControl.clearControlRun();<a name="line.141"></a>
<span class="sourceLineNo">142</span>        if(spdControl == null) {DebugLog.log(DebugLog.LVL_SEVERE, this, "The speed controller was started when it did not exist!"); return;}<a name="line.142"></a>
<span class="sourceLineNo">143</span>        this.spdControlEnabled = true; spdControl.registerIterableEvent();<a name="line.143"></a>
<span class="sourceLineNo">144</span>    }<a name="line.144"></a>
<span class="sourceLineNo">145</span>    <a name="line.145"></a>
<span class="sourceLineNo">146</span>    /**<a name="line.146"></a>
<span class="sourceLineNo">147</span>     * Sets the speed controller's target - will be used however the speed<a name="line.147"></a>
<span class="sourceLineNo">148</span>     * controller has defined it to be.<a name="line.148"></a>
<span class="sourceLineNo">149</span>     * <a name="line.149"></a>
<span class="sourceLineNo">150</span>     * @param val the value to be sent to the speed controller <a name="line.150"></a>
<span class="sourceLineNo">151</span>     */<a name="line.151"></a>
<span class="sourceLineNo">152</span>    public void setSpeedControlTarget(double val) {<a name="line.152"></a>
<span class="sourceLineNo">153</span>        if(!this.spdControlEnabled) {<a name="line.153"></a>
<span class="sourceLineNo">154</span>            DebugLog.log(DebugLog.LVL_WARN, this, "The speed controller's target was set when it was not active" + (Constants.START_MCONTROL_ON_TARGETSET ? "- enabling..." : "."));<a name="line.154"></a>
<span class="sourceLineNo">155</span>            if(Constants.START_MCONTROL_ON_TARGETSET) {this.startSpeedControl(val); return;}<a name="line.155"></a>
<span class="sourceLineNo">156</span>        }<a name="line.156"></a>
<span class="sourceLineNo">157</span>        this.spdControl.setControlTarget(val);<a name="line.157"></a>
<span class="sourceLineNo">158</span>    }<a name="line.158"></a>
<span class="sourceLineNo">159</span>    <a name="line.159"></a>
<span class="sourceLineNo">160</span>    /**<a name="line.160"></a>
<span class="sourceLineNo">161</span>     * Inverts the motor; clockwise becomes counterclockwise and vice versa.<a name="line.161"></a>
<span class="sourceLineNo">162</span>     */<a name="line.162"></a>
<span class="sourceLineNo">163</span>    public void reverseMotor() {this.motorReversed = !this.motorReversed;}<a name="line.163"></a>
<span class="sourceLineNo">164</span><a name="line.164"></a>
<span class="sourceLineNo">165</span>    /**<a name="line.165"></a>
<span class="sourceLineNo">166</span>     * Sets the power scalar for the motor<a name="line.166"></a>
<span class="sourceLineNo">167</span>     * <a name="line.167"></a>
<span class="sourceLineNo">168</span>     * @param pwrScale the power scalar from 0 to 1 to be used for speeds<a name="line.168"></a>
<span class="sourceLineNo">169</span>     */<a name="line.169"></a>
<span class="sourceLineNo">170</span>    public void setPowerScalar(double pwrScale) {<a name="line.170"></a>
<span class="sourceLineNo">171</span>        if(pwrScale &lt; 0 || pwrScale &gt; 1) {DebugLog.log(DebugLog.LVL_SEVERE, this, "The power scalar was set to an illegal value: " + pwrScale + "; scalar not updated!"); return;}<a name="line.171"></a>
<span class="sourceLineNo">172</span>        this.powerScalar = pwrScale;<a name="line.172"></a>
<span class="sourceLineNo">173</span>    }<a name="line.173"></a>
<span class="sourceLineNo">174</span>    <a name="line.174"></a>
<span class="sourceLineNo">175</span>    /**<a name="line.175"></a>
<span class="sourceLineNo">176</span>     * <a name="line.176"></a>
<span class="sourceLineNo">177</span>     * @return the current power scalar<a name="line.177"></a>
<span class="sourceLineNo">178</span>     */<a name="line.178"></a>
<span class="sourceLineNo">179</span>    public double getPowerScalar() {return this.powerScalar;}<a name="line.179"></a>
<span class="sourceLineNo">180</span><a name="line.180"></a>
<span class="sourceLineNo">181</span>    /**<a name="line.181"></a>
<span class="sourceLineNo">182</span>     * <a name="line.182"></a>
<span class="sourceLineNo">183</span>     * @return whether or not the motor is reversed<a name="line.183"></a>
<span class="sourceLineNo">184</span>     */<a name="line.184"></a>
<span class="sourceLineNo">185</span>    public boolean getMotorReversed() {return this.motorReversed;}<a name="line.185"></a>
<span class="sourceLineNo">186</span>    private double reversedCheck(double spd) {return spd * (motorReversed ? -1.0 : 1.0);}<a name="line.186"></a>
<span class="sourceLineNo">187</span>}<a name="line.187"></a>




























































</pre>
</div>
</body>
</html>
